wolf.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
// Enable project-specific definitions and macros
#include "core/internal/config.h"
#include "core/utils/load_core.h"
#include "core/utils/logging.h"
#include "core/utils/string_utils.h"
 
// Folder Registry
#include "core/utils/folder_registry.h"
 
// includes from Eigen lib
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Sparse>
 
#include <libgen.h>
 
// includes from std lib
#include <list>
#include <map>
#include <memory>  // shared_ptr and weak_ptr
#include <set>
 
// System specifics
#include <sys/stat.h>
 
// YAML
#include "yaml-cpp/yaml.h"
#include "yaml-schema-cpp/yaml_conversion.hpp"
 
namespace wolf
{
typedef EIGEN_DEFAULT_DENSE_INDEX_TYPE SizeEigen;
 
typedef std::size_t SizeStd;
 
#define M_TORAD 0.017453292519943295769236907684886127134  // pi / 180
#define M_TODEG 57.295779513082320876798154814105170332    // 180 / pi
 
namespace Constants
{
// Wolf standard tolerance
const double EPS = 1e-8;
// Wolf smmmmall tolerance
const double EPS_SMALL = 1e-16;
}  // namespace Constants
 
}  // namespace wolf
 
// Construct types for any scalar defined in the typedef double above
 
namespace Eigen  // Eigen namespace extension
{
// 1. Vectors and Matrices
typedef Matrix<double, 1, 1>   Matrix1d;   
typedef Matrix<double, 5, 5>   Matrix5d;   
typedef Matrix<double, 6, 6>   Matrix6d;   
typedef Matrix<double, 7, 7>   Matrix7d;   
typedef Matrix<double, 8, 8>   Matrix8d;   
typedef Matrix<double, 9, 9>   Matrix9d;   
typedef Matrix<double, 10, 10> Matrix10d;  
typedef Matrix<double, 1, 1>   Vector1d;   
typedef Matrix<double, 5, 1>   Vector5d;   
typedef Matrix<double, 6, 1>   Vector6d;   
typedef Matrix<double, 7, 1>   Vector7d;   
typedef Matrix<double, 8, 1>   Vector8d;   
typedef Matrix<double, 9, 1>   Vector9d;   
typedef Matrix<double, 10, 1>  Vector10d;  
 
// 2. Sparse matrix
typedef SparseMatrix<double> SparseMatrixd;
 
// 3. Row major matrix
typedef Matrix<double, Dynamic, Dynamic, RowMajor> MatrixRowXd;  
}  // namespace Eigen
 
namespace wolf
{
 
template <int Size, int DesiredSize>
struct StaticSizeCheck
{
    template <typename T>
    StaticSizeCheck(const T&)
    {
        static_assert(Size == DesiredSize, "Size of static Vector or Matrix does not match");
    }
};
//
template <int DesiredSize>
struct StaticSizeCheck<Eigen::Dynamic, DesiredSize>
{
    template <typename T>
    StaticSizeCheck(const T& t)
    {
        assert(t == DesiredSize && "Size of dynamic Vector or Matrix does not match");
    }
};
//
template <int DesiredR, int DesiredC>
struct MatrixSizeCheck
{
    template <typename T>
    static void check(const T& t)
    {
        StaticSizeCheck<T::RowsAtCompileTime, DesiredR>(t.rows());
        StaticSizeCheck<T::ColsAtCompileTime, DesiredC>(t.cols());
    }
};
 
//
// End of check matrix sizes /////////////////////////////////////////////////
 
//      TYPEDEFS FOR POINTERS, LISTS AND ITERATORS IN THE WOLF TREE
 
#define WOLF_DECLARED_PTR_TYPEDEFS(Name)                                                                              \
    typedef std::shared_ptr<Name>       Name##Ptr;                                                                    \
    typedef std::shared_ptr<const Name> Name##ConstPtr;                                                               \
    typedef std::weak_ptr<Name>         Name##WPtr;                                                                   \
    typedef std::weak_ptr<const Name>   Name##ConstWPtr;
 
#define WOLF_DECLARED_LIST_TYPEDEFS(Name)                                                                             \
    typedef std::list<Name##Ptr>                  Name##PtrList;                                                      \
    typedef Name##PtrList::iterator               Name##PtrListIter;                                                  \
    typedef Name##PtrList::const_iterator         Name##PtrListConstIter;                                             \
    typedef Name##PtrList::reverse_iterator       Name##PtrListRevIter;                                               \
    typedef std::list<Name##WPtr>                 Name##WPtrList;                                                     \
    typedef Name##WPtrList::iterator              Name##WPtrListIter;                                                 \
    typedef Name##WPtrList::const_iterator        Name##WPtrListConstIter;                                            \
    typedef Name##WPtrList::reverse_iterator      Name##WPtrListRevIter;                                              \
    typedef std::list<Name##ConstPtr>             Name##ConstPtrList;                                                 \
    typedef Name##ConstPtrList::iterator          Name##ConstPtrListIter;                                             \
    typedef Name##ConstPtrList::const_iterator    Name##ConstPtrListConstIter;                                        \
    typedef Name##ConstPtrList::reverse_iterator  Name##ConstPtrListRevIter;                                          \
    typedef std::list<Name##ConstWPtr>            Name##ConstWPtrList;                                                \
    typedef Name##ConstWPtrList::iterator         Name##ConstWPtrListIter;                                            \
    typedef Name##ConstWPtrList::const_iterator   Name##ConstWPtrListConstIter;                                       \
    typedef Name##ConstWPtrList::reverse_iterator Name##ConstWPtrListRevIter;
 
#define WOLF_DECLARED_SET_TYPEDEFS(Name)                                                                              \
    typedef std::set<Name##Ptr>                  Name##PtrSet;                                                        \
    typedef Name##PtrSet::iterator               Name##PtrSetIter;                                                    \
    typedef Name##PtrSet::const_iterator         Name##PtrSetConstIter;                                               \
    typedef Name##PtrSet::reverse_iterator       Name##PtrSetRevIter;                                                 \
    typedef std::set<Name##WPtr>                 Name##WPtrSet;                                                       \
    typedef Name##WPtrSet::iterator              Name##WPtrSetIter;                                                   \
    typedef Name##WPtrSet::const_iterator        Name##WPtrSetConstIter;                                              \
    typedef Name##WPtrSet::reverse_iterator      Name##WPtrSetRevIter;                                                \
    typedef std::set<Name##ConstPtr>             Name##ConstPtrSet;                                                   \
    typedef Name##ConstPtrSet::iterator          Name##ConstPtrSetIter;                                               \
    typedef Name##ConstPtrSet::const_iterator    Name##ConstPtrSetConstIter;                                          \
    typedef Name##ConstPtrSet::reverse_iterator  Name##ConstPtrSetRevIter;                                            \
    typedef std::set<Name##ConstWPtr>            Name##ConstWPtrSet;                                                  \
    typedef Name##ConstWPtrSet::iterator         Name##ConstWPtrSetIter;                                              \
    typedef Name##ConstWPtrSet::const_iterator   Name##ConstWPtrSetConstIter;                                         \
    typedef Name##ConstWPtrSet::reverse_iterator Name##ConstWPtrSetRevIter;
 
#define WOLF_PTR_TYPEDEFS(ClassName)                                                                                  \
    class ClassName;                                                                                                  \
    WOLF_DECLARED_PTR_TYPEDEFS(ClassName);                                                                            \
    WOLF_DECLARED_LIST_TYPEDEFS(ClassName);                                                                           \
    WOLF_DECLARED_SET_TYPEDEFS(ClassName);
 
#define WOLF_STRUCT_PTR_TYPEDEFS(StructName)                                                                          \
    struct StructName;                                                                                                \
    WOLF_DECLARED_PTR_TYPEDEFS(StructName);                                                                           \
    WOLF_DECLARED_LIST_TYPEDEFS(StructName);                                                                          \
    WOLF_DECLARED_SET_TYPEDEFS(StructName);
 
// NodeBase
WOLF_PTR_TYPEDEFS(NodeBase);
 
// NodeStateBlocks
WOLF_PTR_TYPEDEFS(NodeStateBlocks);
 
// Problem
WOLF_PTR_TYPEDEFS(Problem);
 
// Tree Manager
WOLF_PTR_TYPEDEFS(TreeManagerBase);
 
// Hardware
WOLF_PTR_TYPEDEFS(HardwareBase);
 
// - Sensors
WOLF_PTR_TYPEDEFS(SensorBase);
 
// - Processors
WOLF_PTR_TYPEDEFS(ProcessorBase);
 
// - ProcessorMotion
WOLF_PTR_TYPEDEFS(ProcessorMotion);
 
// Trajectory
WOLF_PTR_TYPEDEFS(TrajectoryBase);
 
// - Frame
WOLF_PTR_TYPEDEFS(FrameBase);
 
class TimeStamp;
typedef std::map<TimeStamp, FrameBasePtr>      FramePtrMap;
typedef std::map<TimeStamp, FrameBaseConstPtr> FrameConstPtrMap;
 
// - Capture
WOLF_PTR_TYPEDEFS(CaptureBase);
 
// - Feature
WOLF_PTR_TYPEDEFS(FeatureBase);
 
// - Factor
WOLF_PTR_TYPEDEFS(FactorBase);
 
// Map
WOLF_PTR_TYPEDEFS(MapBase);
 
// - Landmark
WOLF_PTR_TYPEDEFS(LandmarkBase);
 
// - - State blocks
WOLF_PTR_TYPEDEFS(StateBlock);
WOLF_PTR_TYPEDEFS(StatePoint2d);
WOLF_PTR_TYPEDEFS(StatePoint3d);
WOLF_PTR_TYPEDEFS(StateAngle);
WOLF_PTR_TYPEDEFS(StateQuaternion);
 
// - - Local Parametrization
WOLF_PTR_TYPEDEFS(LocalParametrizationBase);
 
// ==================================================
// Some dangling functions
 
inline bool file_exists(const std::string& _name)
{
    struct stat buffer;
    return (stat(_name.c_str(), &buffer) == 0);
}
 
inline const Eigen::Vector3d gravity(void)
{
    return Eigen::Vector3d(0, 0, -9.806);
}
 
// ===================================================
// Schema folder registry
WOLF_REGISTER_FOLDER("core", _WOLF_SCHEMA_DIR);
 
}  // namespace wolf